#include "transformation.h"

matrix::matrix(unsigned m): M(new float *[m]), len(m){
	
	register unsigned i, j;
		
	for(i = 0; i < len; i++){
		M[i] = new float[len];
		for(j = 0; j < len; j++)
			M[i][j] = INITIAL_VALUE;
	}

}

matrix::matrix(matrix &Ma): M(new float *[Ma.len]), len(Ma.len){

	register unsigned i;
	
	for(i = 0; i < len; i++){
		M[i] = new float[len];
		memcpy(M[i], Ma.M[i], sizeof(float) * len);
	}

}

matrix::~matrix(){
	
	register unsigned i;

	for(i = 0; i < len; i++)	
		delete[] M[i];
	delete[] M;

}

matrix &matrix::transposed(){
	
	register unsigned i, j;
	float buff;

	for(i = 0; i < len; i++)
		for(j = 0; j < len; j++){
			buff = M[i][j];
			M[i][j] = M[j][i];
			M[j][i] = buff;
		}
	return *this;

}

matrix &matrix::multiply(matrix &Ma){

	register unsigned i, j, k;
	matrix buff(len);
	
	for(i = 0; i < len; i++)
		for(j = 0; j < len; j++)
			for(k = 0; k < len; k++)
				buff[i][j] += M[i][k] * Ma[k][j];
	return *this = buff;

}

matrix &matrix::operator=(matrix &Ma){

	register unsigned i;
	
	for(i = 0; i < len; i++)
		memcpy(M[i], Ma.M[i], sizeof(float) * len);
	return *this;

}

float *matrix::operator[](unsigned i){

	return M[i];

}

unsigned matrix::getLen(){

	return len;

}

std::ostream &operator<<(std::ostream &out, matrix &Ma){

	unsigned i, j;
	
	for(i = 0; i < Ma.getLen(); i++){
		for(j = 0; j < Ma.getLen(); j++)
			out << std::fixed << std::setprecision(OUTPUT_PRECISION) << Ma[i][j] << ' ';
		out << std::endl;
	}
	return out;

}

transformation::transformation(): matrix(DIMENSION + 1){

	register unsigned i;

	for(i = 0; i < DIMENSION + 1; i++)	
		(*this)[i][i] = 1.0;

}

transformation &transformation::translation(float dx, float dy){

	transformation Ma;
	
	Ma[2][0] = dx;
	Ma[2][1] = dy;
	multiply(Ma);
	return *this;

}

transformation &transformation::rotation(float angle, float x, float y){

	transformation Ma;
	float cosValue = cos(angle);
	float sinValue = sin(angle);
	
	Ma[0][0] = cosValue;
	Ma[0][1] = sinValue;
	Ma[1][0] = -sinValue;
	Ma[1][1] = cosValue;
	translation(-x, -y);
	multiply(Ma);
	translation(x, y);
	return *this;

}

transformation &transformation::scaling(float sx, float sy, float x, float y){

	transformation Ma;
	
	Ma[0][0] = sx;
	Ma[1][1] = sy;
	translation(-x, -y);
	multiply(Ma);
	translation(x, y);
	return *this;

}

float *transformation::multiplyVector(float *v){

	float buff;
	
	buff = v[0] * (*this)[0][0] + v[1] * (*this)[1][0] + v[2] * (*this)[2][0];
	v[1] = v[0] * (*this)[0][1] + v[1] * (*this)[1][1] + v[2] * (*this)[2][1];
	v[0] = buff;
	return v;

}

gTypeGo &transformation::multiplyGTypeGo(gTypeGo &go){
	
	float v[3];
	bool flag = false;
	
	v[2] = 1;
	if(go.getTypeValue() == TYPE_ARC_CLOCK || go.getTypeValue() == TYPE_ARC_COUNTER){
		v[0] = go[0] + go[3];
		v[1] = go[1] + go[4];
		multiplyVector(v);
		go[3] = v[0];
		go[4] = v[1];
		flag = true;
	}
	v[0] = go[0];
	v[1] = go[1];
	multiplyVector(v);
	go[0] = v[0];
	go[1] = v[1];
	if(flag){
		go[3] = go[3] - go[0];
		go[4] = go[4] - go[1];
	}
	return go;

}

void transformation::clear(){

	register unsigned i, j;
	
	for(i = 0; i < DIMENSION + 1; i++)
		for(j = 0; j < DIMENSION + 1; j++)
			if(i == j)
				(*this)[i][j] = 1.0;
			else
				(*this)[i][j] = 0.0;
				
}

gObject &transformation::apply(gObject &object){

	gObject::iterator iter = object.begin();
	
	for(; iter != object.end(); iter++)
		multiplyGTypeGo(*dynamic_cast<gTypeGo *>(*iter));
	return object;

}

gObjectContainer &transformation::apply(gObjectContainer &container){

	gObjectContainer::iterator iter = container.begin();
	
	for(; iter != container.end(); iter++)
		apply(**iter);
	return container;

}

toolTransformation::algorithmError::algorithmError(const char *str) throw(): 
	report(new char[strlen(str) + 1]){

	strcpy(report, str);

}

toolTransformation::algorithmError::~algorithmError() throw(){

	delete [] report;

}

const char *toolTransformation::algorithmError::what() const throw(){

	return report;

}

toolTransformation::toolTransformation(){

	toolRadius = 0;
	depth = 0;
	flag = false;
	commonPoint = true;

}

void toolTransformation::setParam(float r, float z){

	toolRadius = r;
	depth = z;

}

float toolTransformation::getToolRadius() const{

	return toolRadius;

}

float toolTransformation::getDepth() const{

	return depth;

}

float toolTransformation::lineIntersect(float *P, float *N, float *p, float *n){

	float b[2];
	float detT, detT0;
	
	b[0] = N[0] - P[0];
	b[1] = N[1] - P[1];
	detT = n[0] * p[1] - p[0] * n[1];
	detT0 = n[0] * b[1] - b[0] * n[1];
	return detT0 / detT;

}

bool toolTransformation::isLinearlyIndependent(float *p, float *m){
		
	float rel1 = p[0] * m[1];
	float rel2 = p[1] * m[0];
	
	return  rel1 - GAP < rel2 && rel2 < rel1 + GAP ? false : true;
	
}

int toolTransformation::quadraticEquation(float a, float b, float c, float *x){

	float D = b * b - 4 * a * c;
	
	if(D < 0){
		if(D > MAGICAL_NUMBER)
			D = 0;
		else
			return 0;
	}
	if(D - GAP < 0 && 0 < D + GAP){
		x[0] = -b / (2 * a);
		return 1;
	}
	x[0] = (-b + sqrt(D)) / (2 * a);
	x[1] = (-b - sqrt(D)) / (2 * a);
	return 2;
	
}

int toolTransformation::lineArcIntersect(
	float *P, float *p,
	float *S, float r,
	float *t
	){

	float a = p[0] * p[0] 
		+ p[1] * p[1];
	float b = 2 * (P[0] * p[0] - p[0] * S[0] 
		+ P[1] * p[1] - p[1] * S[1]);
	float c = P[0] * P[0] - 2 * S[0] * P[0] + S[0] * S[0]
		+ P[1] * P[1] - 2 * S[1] * P[1] + S[1] * S[1]
		- r * r;
	return quadraticEquation(a, b, c, t);

}

int toolTransformation::arcIntersect(
	float *S0, float r0,
	float *S1, float r1,
	float *P0, float *P1
	){

	float d = sqrt((S0[0] - S1[0]) * (S0[0] - S1[0]) 
		+ (S0[1] - S1[1]) * (S0[1] - S1[1]));
		
	if(d < fabs(r0 - r1) || d > r0 + r1){
		//a nice shit
		if(d + GAP >= fabs(r0 - r1)){
			r1 = r0 - r1 < 0? 
				r1 - GAP:
				r1 + GAP;
		}
		else if(d - GAP <= r0 + r1)
			r1 -= GAP;
		if(d < fabs(r0 - r1) || d > r0 + r1)
			return 0;
	}
	if(d - GAP < r0 + r1 && r0 + r1 < d + GAP){
		//this is it
		P0[0] = S0[0] + r0 * (S1[0] - S0[0]) / d;
		P0[1] = S0[1] + r0 * (S1[1] - S0[1]) / d;
		return 1;
	}
	
	float a = (r0 * r0 - r1 * r1) / (2 * d) + d / 2.;
	float h = sqrt(r0 * r0 - a * a);
	float P2[2];
	
	P2[0] = S0[0] + a / d * (S1[0] - S0[0]);
	P2[1] = S0[1] + a / d * (S1[1] - S0[1]);
	
	P0[0] = P2[0] + h / d * (S0[1] - S1[1]);
	P0[1] = P2[1] + h / d * (S1[0] - S0[0]);
	P1[0] = P2[0] - h / d * (S0[1] - S1[1]);
	P1[1] = P2[1] - h / d * (S1[0] - S0[0]);
	return 2;

}

float toolTransformation::arcAngle(float *S, float *P){
	
	float angle = atan2(P[1] - S[1], P[0] - S[0]);
	
	return angle < 0 ? angle + 2 * M_PI : angle;
	
}

void toolTransformation::getUnitVector(float *M, float *N, float *v){

	float d;

	v[0] = N[0] - M[0];
	v[1] = N[1] - M[1];
	d = sqrt(v[0] * v[0] + v[1] * v[1]);
	if(d - GAP < 0 && d + GAP > 0)
		return;
	v[0] = v[0] / d;
	v[1] = v[1] / d;

}

bool toolTransformation::getNext(gObject &object, gObject::iterator &iter){

	for(iter++; iter != object.end(); iter++){
		gTypeGo &g = dynamic_cast<gTypeGo &>(**iter);
		setZ(g);
		if((*goNext)[0] == g[0] && (*goNext)[1] == g[1])
			overlooked.push_back(&g);
		else
			return true;
	}
	return false;

}

bool toolTransformation::isCommonPoint(gObject &object) const{

	gTypeGo &g0 = dynamic_cast<gTypeGo &>(*object[0]);
	gTypeGo &g1 = dynamic_cast<gTypeGo &>(*object[object.size() - 1]);

	if(g0[0] - GAP < g1[0] && g0[0] + GAP > g1[0] &&
		g0[1] - GAP < g1[1] && g0[1] + GAP > g1[1])
		return true;
	return false;

}

gObject &toolTransformation::apply(gObject &object) throw(algorithmError){

	if(object.size() < 2)
		throw algorithmError(ERROR_LINE_COUNT);

	gObject::iterator iter = object.begin();
	float O[2], P[2];
	bool flag = isCommonPoint(object);

	n[0] = 0;
	n[1] = 0;
	go = 0;
	goNext = dynamic_cast<gTypeGo *>(*iter);
	setZ(*goNext);
	while(getNext(object, iter)){
		ASSIGN_VECTOR(m, n);
		goPrev = go;
		go = goNext;
		goNext = dynamic_cast<gTypeGo *>(*iter);
		ASSIGN_VECTOR(O, *go);
		ASSIGN_VECTOR(P, *goNext);
		getUnitVector(O, P, n);
		setGo();
	}
	m[0] = 0;
	m[1] = 0;
	goPrev = go;
	go = goNext;
	setGo();
	if(flag)
		makeCommonPoint(object);
	return object;

}

void toolTransformation::setGo() throw(algorithmError){

	if(
		(go->getTypeValue() == TYPE_LINEAR_CUT || go->getTypeValue() == TYPE_LINEAR_MOVE) &&
		(goNext->getTypeValue() == TYPE_LINEAR_CUT || goNext->getTypeValue() == TYPE_LINEAR_MOVE)
		)
		goLinearLinear();
	else if(
		(go->getTypeValue() == TYPE_LINEAR_CUT || go->getTypeValue() == TYPE_LINEAR_MOVE) &&
		(goNext->getTypeValue() == TYPE_ARC_CLOCK || goNext->getTypeValue() == TYPE_ARC_COUNTER)
		)
		goLinearArc();
	else if(
		(go->getTypeValue() == TYPE_ARC_CLOCK || go->getTypeValue() == TYPE_ARC_COUNTER) &&
		(goNext->getTypeValue() == TYPE_LINEAR_CUT || goNext->getTypeValue() == TYPE_LINEAR_MOVE)
		)
		goArcLinear();
	else if(
		(go->getTypeValue() == TYPE_ARC_CLOCK || go->getTypeValue() == TYPE_ARC_COUNTER) &&
		(goNext->getTypeValue() == TYPE_ARC_CLOCK || goNext->getTypeValue() == TYPE_ARC_COUNTER)
		)
		goArcArc();
	setOverlooked();

}

void toolTransformation::setOverlooked(){

	if(overlooked.size() == 0)
		return;
	
	std::vector<gTypeGo *>::iterator iter = overlooked.begin();
	for(; iter != overlooked.end(); iter++){
		(**iter)[0] = (*go)[0];
		(**iter)[1] = (*go)[1];
	}
	overlooked.clear();

}

void toolTransformation::goLinearLinear(){

	if(isLinearlyIndependent(m, n)){
		float t;
		float O[2], P[2];
		
		ASSIGN_VECTOR(O, *goPrev);
		LINEAR_NORMAL_INTERPOL(P, *goNext, n, toolRadius);
		t = lineIntersect(O, P, m, n);
		LINEAR_INTERPOL(*go, O, m, t);
	}
	else
		LINEAR_NORMAL_INTERPOL(*go, *go, n, toolRadius);
	
}

float toolTransformation::getRadius(gTypeGo &g, float *S){
	
	float r;

	r = sqrt((S[0] - g[0]) * (S[0] - g[0]) + (S[1] - g[1]) * (S[1] - g[1]));
	if(g.getTypeValue() == TYPE_ARC_CLOCK)
		return r + toolRadius;
	return r - toolRadius;

}

void toolTransformation::goLinearArc() throw(algorithmError){

	gTypeGo &next = *goNext;
	gTypeGo &g = *go;
	float O[2], S[2], t[2];
	float r;
	int x;
	
	S[0] = g[0] + next[3]; 
	S[1] = g[1] + next[4];
	r = getRadius(next, S);
	if(m[0] == 0 && m[1] == 0){
		ASSIGN_VECTOR(O, g);
		getUnitVector(S, O, t);
		LINEAR_INTERPOL(g, S, t, r);
	}
	else{
		ASSIGN_VECTOR(O, *goPrev);
		x = lineArcIntersect(O, m, S, r, t);
		if(!x)
			throw(algorithmError(ERROR_LINE_MET));	
		else if(x == 2){
			ASSIGN_VECTOR(O, *goPrev);
            if(suitPoint(S, m, O, t))
                LINEAR_INTERPOL(g, O, m, t[0]);
            else{
                t[0] = t[1];
				x = 1;
            }
		}
        if(x == 1){
            LINEAR_INTERPOL(g, O, m, t[0]);
        }
	}
	next[3] = S[0] - g[0];
	next[4] = S[1] - g[1];

}

void toolTransformation::goArcLinear() throw(algorithmError){
	
	gTypeGo &next = *goNext;
	gTypeGo &g = *go;
	gTypeGo &prev = *goPrev;
	float O[2], S[2], t[2];
	float r;
	int x;
	
	LINEAR_NORMAL_INTERPOL(O, next, n, toolRadius);
	S[0] = prev[0] + g[3];
	S[1] = prev[1] + g[4];
	r = getRadius(g, S); 
	x = lineArcIntersect(O, n, S, r, t);
	if(!x)
		throw(algorithmError(ERROR_LINE_MET));
	else if(x == 2){
        if(suitPoint(S, n, O, t))
            LINEAR_INTERPOL(g, O, n, t[0]);
        else{
            t[0] = t[1];
            x = 1;
        }
	}
	if(x == 1)
        LINEAR_INTERPOL(g, O, n, t[0]);
	
}

bool toolTransformation::suitPoint(float *S, float *n, float *O, float *t){
	
    float A[2], B0[2], B1[2];

    LINEAR_INTERPOL(B0, O, n, t[0]);
    LINEAR_INTERPOL(B1, O, n, t[1]);
    ASSIGN_VECTOR(A, *go);
    if((DOT_PRODUCT(A, n) - GAP < DOT_PRODUCT(S, n)) &&
       (DOT_PRODUCT(A, n) + GAP > DOT_PRODUCT(S, n))){

        if(go->getTypeValue() == TYPE_LINEAR_CUT || go->getTypeValue() == TYPE_LINEAR_MOVE){
            if(fabs((B0[0] - (*goPrev)[0]) / n[0]) < fabs((B1[0] - (*goPrev)[0]) / n[0]))
                return true;
        }
        else if(fabs((B0[0] - (*goNext)[0]) / n[0]) < fabs((B1[0] - (*goNext)[0]) / n[0]))
            return true;
        return false;
    }
    else if((DOT_PRODUCT(A, n) < DOT_PRODUCT(S, n) && DOT_PRODUCT(B0, n) < DOT_PRODUCT(S, n)) ||
        ((DOT_PRODUCT(A, n) > DOT_PRODUCT(S, n) && DOT_PRODUCT(B0, n) > DOT_PRODUCT(S, n))))
        return true;
    return false;

}

bool toolTransformation::nearPoint(float *S, float *n, float *B0, float *B1){
	
    float A[2];

    ASSIGN_VECTOR(A, *go);
    if((DOT_PRODUCT(A, n) - GAP < DOT_PRODUCT(S, n)) && (DOT_PRODUCT(A, n) + GAP > DOT_PRODUCT(S, n))){
        float a, b0, b1;

        ASSIGN_VECTOR(A, *goPrev);
        a = atan2(A[1] - S[1], A[0] - S[0]);
        b0 = atan2(B0[1] - S[1], B0[0] - S[0]);
        if(b0 < a)
            b0 += 2 * M_PI;
        b1 = atan2(B1[1] - S[1], B1[0] - S[0]);
        if(b1 < a)
            b1 += 2 * M_PI;
        if(go->getTypeValue() == TYPE_ARC_COUNTER && b0 < b1)
            return true;
        else if(go->getTypeValue() == TYPE_ARC_CLOCK && b0 > b1)
            return true;
        return false;

    }
    else if((DOT_PRODUCT(A, n) < DOT_PRODUCT(S, n) && DOT_PRODUCT(B0, n) < DOT_PRODUCT(S, n)) ||
        ((DOT_PRODUCT(A, n) > DOT_PRODUCT(S, n) && DOT_PRODUCT(B0, n) > DOT_PRODUCT(S, n))))
        return true;
    return false;

}

void toolTransformation::goArcArc() throw(algorithmError){
	
        gTypeGo &next = *goNext;
	gTypeGo &g = *go;
	gTypeGo &prev = *goPrev;
	float P0[2], P1[2], S0[2], S1[2], t[2];
	float r0, r1;
	int x;
	
	S0[0] = prev[0] + g[3];
	S0[1] = prev[1] + g[4];
 	r0 = getRadius(g, S0);
	if(m[0] == 0 && m[1] == 0){
		ASSIGN_VECTOR(P0, g);
		getUnitVector(S0, P0, m);
		LINEAR_INTERPOL(g, S0, m, r0);
		ASSIGN_VECTOR(S1, S0);
		go = goPrev;
	}
	else{
		S1[0] = g[0] + next[3];
		S1[1] = g[1] + next[4];
		r1 = getRadius(next, S1);
		if(r0 - GAP < r1 && r0 + GAP > r1 &&
			S0[0] - GAP  < S1[0] && S0[0] + GAP > S1[0] &&
			S0[1] - GAP  < S1[1] && S0[1] + GAP > S1[1]){
			
			ASSIGN_VECTOR(t, g);
			getUnitVector(S0, t, P1);
			LINEAR_INTERPOL(P0, S0, P1, r0);
			x = 1;
		}
		else
			x = arcIntersect(S0, r0, S1, r1, P0, P1);
		if(!x)
			throw(algorithmError(ERROR_CIRCLES_MET));
		else if(x == 2){
            t[0] = S1[1] - S0[1];
            t[1] = S0[0] - S1[0];

            if(nearPoint(S0, t, P0, P1))
                ASSIGN_VECTOR(g, P0);
            else{
                ASSIGN_VECTOR(P0, P1);
                x = 1;
            }
		}
		if(x == 1)
			ASSIGN_VECTOR(g, P0);
	}
	next[3] = S1[0] - (*go)[0];
	next[4] = S1[1] - (*go)[1];
	
}

gObjectContainer &toolTransformation::apply(gObjectContainer &container) throw(algorithmError){

	gObjectContainer::iterator iter = container.begin();
	
	for(; iter != container.end(); iter++)
		apply(**iter);
	return container;


}

void toolTransformation::setMoves(bool boolean){

	flag = boolean;

}

bool toolTransformation::getMoves() const{

	return flag;

}

void toolTransformation::setCommonPoint(bool boolean){

	commonPoint = boolean;

}

bool toolTransformation::getCommonPoint() const{

	return commonPoint;

}

void toolTransformation::setZ(gTypeGo &g){

	if(flag || g.getTypeValue() != TYPE_LINEAR_MOVE)
		g[2] += depth;

}

void toolTransformation::makeCommonPoint(gObject &object) throw(algorithmError){
	
	if(commonPoint){
		gObject::iterator iter = object.begin() + object.getIndexByObject(goPrev);
		float buffRadius = toolRadius;
		float buffDepth = depth;
		float O[2], P[2]; 
		
		goNext = goPrev;
		toolRadius = depth = 0;
		if(!getNext(object, iter))
			throw algorithmError(ERROR_EXCEED);
		overlooked.clear();
		go =  dynamic_cast<gTypeGo *>(*iter);
		if(getNext(object, iter))
			throw algorithmError(ERROR_EXCEED);
		iter = object.begin();
		gTypeGo &goBegin = dynamic_cast<gTypeGo &>(**iter);
		overlooked.push_back(&goBegin);
		ASSIGN_VECTOR(O, goBegin);
		if(!getNext(object, iter))
			throw algorithmError(ERROR_EXCEED);
		goNext = dynamic_cast<gTypeGo *>(*iter);
		if(goNext->getTypeValue() == TYPE_ARC_CLOCK || 
			goNext->getTypeValue() == TYPE_ARC_COUNTER){
			//recount the middle of a second circle
			(*goNext)[3] = goBegin[0] + (*goNext)[3] - (*go)[0];
			(*goNext)[4] = goBegin[1] + (*goNext)[4] - (*go)[1];
		}
		ASSIGN_VECTOR(m, n);
		ASSIGN_VECTOR(P, *goNext);
		getUnitVector(O, P, n);
		setGo();
		toolRadius = buffRadius;
		depth = buffDepth;
	}

}

morphTransformation::morphTransformation(){

	distance = 0;
	point[0] = point[1] = 0;
	go = NULL;
	next = true;
	commonPoint = false;
	
}

void morphTransformation::setNext(bool boolean){

	next = boolean;

}

bool morphTransformation::isNext() const{

	return next;

}

void morphTransformation::setType(morphType way){

	type = way;
	
}

void morphTransformation::setDistance(float d){
	
	distance = d;
	
}

void morphTransformation::setMove(float x, float y){

	point[0] = x;
	point[1] = y;

}

morphTransformation::morphType morphTransformation::getType() const{

	return type;

}

float morphTransformation::getDistance() const{

	return distance;

}

void morphTransformation::getPoint(float *p) const{

	p[0] = point[0];
	p[1] = point[1];

}

void morphTransformation::transformMove() throw(morphError){

	//after after

}

bool morphTransformation::isLinear(gTypeGo &g) const{

	if(g.getTypeValue() == TYPE_LINEAR_MOVE ||
		g.getTypeValue() == TYPE_LINEAR_CUT)
		
		return true;
	return false;

}
	
bool morphTransformation::isArc(gTypeGo &g) const{

	if(g.getTypeValue() == TYPE_ARC_COUNTER ||
		g.getTypeValue() == TYPE_ARC_CLOCK)
		
		return true;
	return false;

}

void morphTransformation::assignVector(float *P){

	std::vector<gTypeGo *>::iterator iter = group.begin();

	ASSIGN_VECTOR(*go, P);
	for(; iter != group.end(); iter++)
		ASSIGN_VECTOR(**iter, P);

}

void morphTransformation::transformDistance() throw(morphError){
	
	gTypeGo *buff;
	iterNext = getNext();
	int pos = group.size() - 1;
	
	iterPrev = getPrev();
	if(group.size()){
		if(iterPrev != object->end()){
			//swap
			if(pos != (int) group.size() - 1){
				buff = go;
				go = group[group.size() - 1];
				group[group.size() - 1] = buff;
			}
		}
		else if(pos > -1){
			buff = group[pos];
			group[pos] = go;
			go = buff;
		}
	}
	//commonPoint
	if(commonPoint){
		//start
		if(iterPrev == object->end()){
			group.push_back(go);
			iter = object->end() - 1;
			go = dynamic_cast<gTypeGo *>(*iter);
			pos = group.size();
			iterPrev = getPrev();
			//swap
			if(pos < (int) group.size()){
				buff = go;
				go = group[group.size() - 1];
				group[group.size() - 1] = buff;
			}
		}
		//end
		if(iterNext == object->end()){
			buff = go;
			iter = object->begin();
			go = dynamic_cast<gTypeGo *>(*iter);
			group.push_back(go);
			iterNext = getNext();
			go = buff;
		}
	}
	
	float buffP0[2], P0[2], P1[2];
	
	clear();
	ASSIGN_VECTOR(P0, *go);
	ASSIGN_VECTOR(buffP0, P0);
	if(iterPrev == object->end()){
		//begin
		gTypeGo &goNext = dynamic_cast<gTypeGo &>(**iterNext);
		
		ASSIGN_VECTOR(P1, goNext);
		if(isArc(goNext)){
			P1[0] = P0[0] + goNext[3];
			P1[1] = P0[1] + goNext[4];
		}
		interpolPoint(P0, P1, goNext.getTypeValue());
		//ASSIGN_VECTOR(*go, P0);
		assignVector(P0);
		if(isArc(goNext)){
			goNext[3] = P1[0] - P0[0];
			goNext[4] = P1[1] - P0[1];
		}
	}
	else if(iterNext == object->end()){
		//end
		gTypeGo &goPrev = dynamic_cast<gTypeGo &>(**iterPrev);
		
		ASSIGN_VECTOR(P1, goPrev);
		if(isArc(*go)){	 
			P1[0] = goPrev[0] + (*go)[3];
			P1[1] = goPrev[1] + (*go)[4];
		}
		interpolPoint(P0, P1, go->getTypeValue());
		//ASSIGN_VECTOR(*go, P0);
		assignVector(P0);
	}
	else{
		if(next){
			//n
			gTypeGo &goNext = dynamic_cast<gTypeGo &>(**iterNext);
			
			ASSIGN_VECTOR(P1, goNext);
			if(isArc(goNext)){
				P1[0] = P0[0] + goNext[3];
				P1[1] = P0[1] + goNext[4];
			}
			if(isArc(*go)){
				//K
				interpolPoint(P0, P1, goNext.getTypeValue());
				//ASSIGN_VECTOR(*go, P0);
				assignVector(P0);
				translation(P0[0] - buffP0[0], P0[1] - buffP0[1]);
				movePrev();
			}
			else if(isLinear(*go)){
				//L
				interpolPoint(P0, P1, goNext.getTypeValue());
				//ASSIGN_VECTOR(*go, P0);
				assignVector(P0);
			}
			if(isArc(goNext)){
				goNext[3] = P1[0] - P0[0];
				goNext[4] = P1[1] - P0[1];
			}
		}
		else{
			//m
			gTypeGo &goNext = dynamic_cast<gTypeGo &>(**iterNext);
			gTypeGo &goPrev = dynamic_cast<gTypeGo &>(**iterPrev);
			
			ASSIGN_VECTOR(P1, goPrev);
			if(isArc(*go)){
				P1[0] = goPrev[0] + (*go)[3];
				P1[1] = goPrev[1] + (*go)[4];
			}
			if(isArc(goNext)){
				//mK
				interpolPoint(P0, P1, go->getTypeValue());
				//ASSIGN_VECTOR(*go, P0);
				assignVector(P0);
				translation(P0[0] - buffP0[0], P0[1] - buffP0[1]);
				moveNext();
			}
			else if(isLinear(goNext)){
				//mL
				interpolPoint(P0, P1, go->getTypeValue());
				//ASSIGN_VECTOR(*go, P0);
				assignVector(P0);
			}
		}
	}
	group.clear();
	
}

bool morphTransformation::isCommonPoint() const{

	return commonPoint;

}

void morphTransformation::setCommonPoint(bool boolean){

	commonPoint = boolean;

}

void morphTransformation::interpolPoint(float *p0, float *p1, int type){

	if(type == TYPE_ARC_CLOCK || type ==  TYPE_ARC_COUNTER){
		float r = DISTANCE(p0, p1);
		float angle = atan2(p0[1] - p1[1], p0[0] - p1[0]);
		
		angle += distance / r;
		p0[0] = p1[0] + r * cos(angle);
		p0[1] = p1[1] + r * sin(angle);
	}
	else if(type == TYPE_LINEAR_MOVE || type == TYPE_LINEAR_CUT){
		float n[2];
		
		toolTransformation::getUnitVector(p0, p1, n);
		LINEAR_INTERPOL(p0, p0, n, distance);
	}
	
}

void morphTransformation::movePrev(){

	int i = iterPrev - object->begin();
		
	for(; i > -1; i--){
		gTypeGo &g = dynamic_cast<gTypeGo &>(*object->at(i));
		if(isArc(g))
			multiplyGTypeGo(g);
		else{
			multiplyGTypeGo(g);
			return;
		}
	}

}

void morphTransformation::moveNext(){

	unsigned i = iterNext - object->begin();

	for(; i < object->size(); i++){
		gTypeGo &g = dynamic_cast<gTypeGo &>(*object->at(i));
		if(isArc(g))
			multiplyGTypeGo(g);
		else
			return;
	}

}

gObject::iterator morphTransformation::getPrev(){

	int i = iter - object->begin();
	
	for(i--; i > -1; i--){
		gTypeGo &g = dynamic_cast<gTypeGo &>(*object->at(i));
		if(!COMPARE(*go, g))
			return i + object->begin();
		else
			group.push_back(&g);
	}
	return object->end();
	
}

gObject::iterator morphTransformation::getNext(){
	
	unsigned i = iter - object->begin();

	for(i++; i < object->size(); i++){
		gTypeGo &g = dynamic_cast<gTypeGo &>(*object->at(i));
		if(!COMPARE(*go, g))
			return i + object->begin();
		else
			group.push_back(&g);
	}
	return object->end();

}

void morphTransformation::apply(gObject *obj, gTypeGo *g) throw(morphError){

	object = obj;
	go = g;
	iter = object->begin() + object->getIndexByObject(go);
	switch(type){
		case DISTANCE:
			transformDistance();
			break;
		case MOVE:
			transformMove();
			break;
	}

}
